
#include "mapping/2d/probability_grid.h"

#include <limits>
#include <Eigen/Core>

#include "mapping/probability_values.h"

namespace cartographer {
namespace mapping {

using std::cout;
using std::endl;

ProbabilityGrid::ProbabilityGrid(const MapLimits &limits)
    : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost) {}

ProbabilityGrid::ProbabilityGrid(const ProbabilityGrid &grid) : Grid2D(grid) {}

// Sets the probability of the cell at 'cell_index' to the given
// 'probability'. Only allowed if the cell was unknown before.
void ProbabilityGrid::SetProbability(const Eigen::Array2i &cell_index, float probability) {
    std::unique_lock<mutex_t> lck(cells_mutex_);
    uint16_t                 &cell = (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
    cell                           = CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
}

// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
// to the probability of the cell at 'cell_index' if the cell has not already
// been updated. Multiple updates of the same cell will be ignored until
// FinishUpdate() is called. Returns true if the cell was updated.
//
// If this is the first call to ApplyOdds() for the specified cell, its value
// will be set to probability corresponding to 'odds'.
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i &cell_index, const std::vector<uint16_t> &table) {
    std::unique_lock<mutex_t> lck(cells_mutex_);

    const int flat_index = ToFlatIndex(cell_index);
    uint16_t *cell       = &(*mutable_correspondence_cost_cells())[flat_index];
    uint16_t  last_cell  = *cell;
    if (*cell >= kUpdateMarker) {
        return false;
    }
    mutable_update_indices()->push_back(flat_index);
    *cell = table[*cell];
    mutable_known_cells_box()->extend(cell_index.matrix());

    // 只对状态变化的栅格进行更新
    // 保存更新的栅格索引(known_cells_box区域)
    if ((*cell - kUpdateMarker) != last_cell) {
        // auto known_cell_index = cell_index - known_cells_box_.min().array();
        // int  known_flat_index = known_cell_index.y() * (known_cells_box_.sizes().x() + 1) + known_cell_index.x();
        update_grid_cells_queue_.push(cell_index);
    }

    return true;
}

// Returns the probability of the cell with 'cell_index'.
float ProbabilityGrid::GetProbability(const Eigen::Array2i &cell_index) const {
    if (!limits().Contains(cell_index))
        return kMinProbability;
    return CorrespondenceCostToProbability(
        ValueToCorrespondenceCost(correspondence_cost_cells()[ToFlatIndex(cell_index)]));
}

std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() {
    Eigen::Array2i offset;
    CellLimits     cell_limits;
    ComputeCroppedLimits(&offset, &cell_limits);
    const double                     resolution = limits().resolution();
    const Eigen::Vector2d            max        = limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
    std::unique_ptr<ProbabilityGrid> cropped_grid =
        std::make_unique<ProbabilityGrid>(MapLimits(resolution, max, cell_limits));

    for (const Eigen::Array2i &xy_index : XYIndexRangeIterator(cell_limits)) {
        auto index = xy_index + offset;
        if (IsOutOfBounds(index))
            continue;
        cropped_grid->mutable_known_cells_box()->extend(xy_index.matrix());
        if (!IsKnown(index))
            continue;
        cropped_grid->SetProbability(xy_index, GetProbability(index));
    }

    std::cout << "old grid correspondence_cost_cells_: " << correspondence_cost_cells_.size() << std::endl;
    std::cout << "new correspondence_cost_cells_: " << cropped_grid->correspondence_cost_cells_.size() << std::endl;

    return std::unique_ptr<Grid2D>(cropped_grid.release());
}

}  // namespace mapping
}  // namespace cartographer
